/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "OLED.h"
#include "mpu6050.h"
#include "inv_mpu.h"


void LED(uint8_t state)
{
  // HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,(GPIO_PinState)state);
  LED_GPIO_Port->BSRR=state?LED_Pin:(LED_Pin<<16U);
}

void LED_Tg()
{
  // HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
  LED_GPIO_Port->BSRR=LED_GPIO_Port->ODR&LED_Pin?(LED_Pin<<16U):LED_Pin;
}

void AIN1(uint8_t state)
{
  GPIOB->BSRR=state?LEFT_MOTOR_A_Pin:(LEFT_MOTOR_A_Pin<<16U);
  // HAL_GPIO_WritePin(GPIOB,LEFT_MOTOR_A_Pin ,(GPIO_PinState)state);
}

void AIN2(uint8_t state)
{
  GPIOB->BSRR=state?LEFT_MOTOR_B_Pin:(LEFT_MOTOR_B_Pin<<16U);
  // HAL_GPIO_WritePin(GPIOB,LEFT_MOTOR_B_Pin ,(GPIO_PinState)state);
}

void BIN1(uint8_t state)
{
  GPIOB->BSRR=state?RIGHT_MOTOR_A_Pin:(RIGHT_MOTOR_A_Pin<<16U);
  // HAL_GPIO_WritePin(GPIOB,RIGHT_MOTOR_A_Pin ,(GPIO_PinState)state);
}

void BIN2(uint8_t state)
{
  GPIOB->BSRR=state?RIGHT_MOTOR_B_Pin:(RIGHT_MOTOR_B_Pin<<16U);
  // HAL_GPIO_WritePin(GPIOB,RIGHT_MOTOR_B_Pin ,(GPIO_PinState)state);
}

void Motor_CompareValue_Set(int16_t Left_Motor_Value,int16_t Right_Motor_Value)
{
    if(Left_Motor_Value>=0)
    {
        AIN1(0);
        AIN2(1);

        TIM1->CCR1=Left_Motor_Value;
        // __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,Left_Motor_Value);
    }
    else
    {
        AIN1(1);
        AIN2(0);

        TIM1->CCR1=-Left_Motor_Value;
        // __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,Left_Motor_Value);
    }

    if(Right_Motor_Value>=0)
    {
        BIN1(0);
        BIN2(1);
        
        TIM1->CCR2=Right_Motor_Value;
        // __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Right_Motor_Value);
    }
    else
    {
        BIN1(1);
        BIN2(0);

        TIM1->CCR2=-Right_Motor_Value;
        // __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,Right_Motor_Value);
    }
}

void Read_Speed(int16_t* Left_Speed_ptr,int16_t* Right_Speed_ptr)
{
  *Left_Speed_ptr=TIM3->CNT;
  TIM3->CNT=0;

  *Right_Speed_ptr=-(TIM4->CNT);
  TIM4->CNT=0;
}

float Med_Angle=-3;	//机械中值。---在这里修改你的机械中值即可。
float Target_Speed=0;	//期望速度（俯仰或者横滚）。---二次开发接口，用于控制小车前进后退及其速度。
#define MAX_PWM 7200
#define MIN_PWM -7200

float 
	Vertical_Kp=-400.0*0.6,//直立环KP,300，调到差不多离机械中值2度(离的度数越近它后期偏的角度就不会那么左右摇摆)左右便有一股力即可—大幅度低频震荡(就是左右摇摆)
	Vertical_Kd=-1.3*0.6;//直立环KD，-0.7，KD增大过程中，慢慢的低频震荡消失了出现平衡，然后调到离3度左右就会出现一会小幅度高频震荡(不是左右摇摆，是那种很快的颤抖)
//最后同时让参数乘0.6  =540  1.9，车子出现一定平衡姿态(不抖)，如果不是平衡状态，证明KD调的不对
float 
	Velocity_Kp=0.25,//速度环KP、KI -2.3 -0.001 取值0-1，这里注意速度KP太高，速度非常快，容易一下子倒地
	Velocity_Ki=0.00001;

void limit(int *left_pwm,int *right_pwm)
{
  *left_pwm=*left_pwm>=MAX_PWM?MAX_PWM:(*left_pwm<=MIN_PWM?MIN_PWM:*left_pwm);

  *right_pwm=*right_pwm>=MAX_PWM?MAX_PWM:(*right_pwm<=MIN_PWM?MIN_PWM:*right_pwm);
}

/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D

入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int PWM_out;
	
	PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
	return PWM_out;
}


/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
	static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;
	float a=0.7;
	
	//1.计算速度偏差
	Encoder_Err=((encoder_left+encoder_right)-Target);//(左电机速度+右电机速度)=测量速度(encoder_left+encoder_right)，Target是期望速度，这里是0
	//2.对速度偏差进行低通滤波
	//low_out=(1-a)*Ek+a*low_out_last;
	EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
	EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度过大的影响直立环的正常工作。
	//3.对速度偏差积分，积分出位移
	Encoder_S+=EnC_Err_Lowout;
	//4.积分限幅
	Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
	
	//5.速度环控制输出计算
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
	return PWM_out;
}



// /*********************
// 转向环：系数*Z轴角速度
// 功能：只负责走直线
// *********************/
// int Turn(int gyro_Z)//这里为什么不传这里偏航角，而使用陀螺仪原始数据gyro_Z，因为陀螺仪偏航角是累积误差的，他会把时间上的误差累积起来
// {
// 	int PWM_out;
	
// 	PWM_out=Turn_Kd*gyro_Z;//速度环极性，先注释掉直立环和速度环的运算，给1和-1，然后沿地旋转，如果有一股阻力则证明调对了极性
// 	return PWM_out;
// }
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
uint32_t count=0;
float Pitch,Roll,Yaw;
short gyrox,gyroy,gyroz;
short aacx,aacy,aacz;
uint8_t ret=0;
char str[30]={0};
int16_t Left_Speed=0,Right_Speed=0,ALL_PWM=0;
int Left_PWM=0,Right_PWM=0;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if(htim->Instance==TIM2)
  {
    count++;

    Read_Speed(&Left_Speed,&Right_Speed);

    ret=mpu_dmp_get_data(&Pitch,&Roll,&Yaw);

    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);

    ALL_PWM=0;

    ALL_PWM=Velocity(0,(int)Left_Speed,(int)Right_Speed);

    ALL_PWM=Vertical(ALL_PWM+Med_Angle,Roll,gyrox);

    Left_PWM=ALL_PWM;
    Right_PWM=ALL_PWM;

    limit(&Left_PWM,&Right_PWM);

    Motor_CompareValue_Set(Left_PWM,Right_PWM);

    if(count>=100)
    {
      LED_Tg();

      count=0;
    }
  }
}
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  /* USER CODE BEGIN 2 */
  OLED_Init();
  MPU_Init();

  while(mpu_dmp_init()!=0)
  {
    HAL_Delay(200);
  }
  

  HAL_TIM_Base_Start_IT(&htim2);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
  HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_2);
  HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_2);
  
  // Motor_CompareValue_Set(1800,900);
  // OLED_ShowString(1,1,"Hello HAL!");

  OLED_Clear();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    if(ret==0)
    {
      sprintf(str,"Roll=%.2f",Roll);
      OLED_ShowString(3,1,str);
      // sprintf(str,"Roll=%.2f",Roll);
      // OLED_ShowString(2,1,str);
      // sprintf(str,"Yaw=%.2f",Yaw);
      // OLED_ShowString(3,1,str);
    }

    sprintf(str,"L_s=%4d",Left_Speed);
    OLED_ShowString(1,1,str);
    sprintf(str,"R_s=%4d",Right_Speed);
    OLED_ShowString(2,1,str);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
